package catchnthrow;

//PID class to be written by you
public class PID {

	// Private attribute containing a reference to the PIDParameters
	// currently used
	private PIDParameters p;
	private final boolean debug = false;

	// Additional attributes
	private double y, yref, v, I, D, ad, bd, yOld;
	private String name;


	// Constructor
	public PID(String name, PIDParameters param) {
		this.name = name;
		setParameters(param);

		ad = p.Td / (p.Td + p.N*p.H);
		bd = p.K*ad*p.N;
		yOld = 0;
	}

	// Calculates the control signal v.
	public synchronized double calculateOutput(double yin, double yrefin) {
		y = yin;
		yref = yrefin;

		v = p.K*(p.Beta*yref - y);

		if (p.Td != 0) {
			D = ad*D - bd*(y - yOld);
		} else {
			D = 0.0;
		}

		if (p.integratorOn) {
			v = v + I + D;
		} else if (p.Td != 0){
			v = v + D;
		}
		
		return v;
	}

	// Updates the controller state. Should use tracking-based anti-windup
	public synchronized void updateState(double u) {
		if (p.integratorOn) {
			I = I + p.H*p.K/p.Ti*(yref - y) + p.H*(1/p.Tr)*(u - v);
		} else {
			I = 0.0;
		}
		yOld = y;
		
		//TODO remove
		if (debug) {
			System.out.printf("%s y: %5.3f, ref: %5.3f, D: %5.3f, I: %5.3f,v: %5.3f \n", name, y ,yref , D , I , v);
		}
	}

	// Returns the sampling interval expressed as a long. Explicit type casting
	// needed
	public synchronized long getHMillis() {
		return (long)(p.H*1000);
	}

	// Sets the PIDParameters. Called from PIDGUI. Must clone newParameters.
	public synchronized void setParameters(PIDParameters newParameters) {
		p = (PIDParameters)newParameters.clone();
		ad = p.Td / (p.Td + p.N*p.H);
		bd = p.K*ad*p.N;
	}
	
	/*
	 * Set the internal state (I, D and yold) of the controller to 0.
	 */
	public synchronized void reset() {
		I = 0.0;
		D = 0.0;
		yOld = 0.0;
	}

	/*
	 * Returns the current PIDParameters.
	 */
	public synchronized PIDParameters getParameters(){
		return p;
	}

}
